# ==================== 系统描述 ====================
# GPS精度为4m
# 加速度计的精度为 1cm/s^2，且有 3cm/s^2 的偏移
# 真实的加速度为 2cm/s^2
# 数据采集频率皆为 10Hz

# ================== 模拟产生数据 ==================
clear all
rng(0);                                 # 设置随机数种子
dt = 0.1;                               # 0.1s采集一次数据
T = 0:dt:100;                           # 0~100s
N = length(T);                          # 采样点数

a = 2e-2;                               # 真实加速度
#s = 1/2 * a * T.^2;                     # 真实位移（实际上不可知）
s = zeros(6, N);
s(1,:) =  1/2 * a * T.^2;
s(2,:) =  1/2 * a * T.^2;
s(3,:) =  1/2 * a * T.^2;
s(4,:) =  a * T;
s(5,:) =  a * T;
s(6,:) =  a * T;

v = a * T;                              # 真实速度

aa = a + 1e-2*randn(size(s)) + 3e-2;    # 加速度计测量数据，有噪声，有偏移1
Rp = 4;                                  # GPS 位置测量精度
Vp = 0.1;                                # GPS 速度测量精度
z(1,:) = s(1,:) + sqrt(Rp)*randn(size(s(1,:)));         # GPS 测量数据，开根号得到
z(2,:) = s(2,:) + sqrt(Rp)*randn(size(s(2,:)));
z(3,:) = s(3,:) + sqrt(Rp)*randn(size(s(3,:)));
z(4,:) = s(4,:) + sqrt(Vp)*randn(size(s(4,:)));
z(5,:) = s(5,:) + sqrt(Vp)*randn(size(s(5,:)));
z(6,:) = s(6,:) + sqrt(Vp)*randn(size(s(6,:)));

R = eye(6, 6);
R(1,1) = Rp * Rp;
R(2,2) = Rp * Rp;
R(3,3) = Rp * Rp;

R(4,4) = Vp * Vp;
R(5,5) = Vp * Vp;
R(6,6) = Vp * Vp;
# ================= 变量定义与初始化 =================
A = [1 0 0 dt 0 0;
     0 1 0 0 dt 0;
     0 0 1 0 0 dt;
     0 0 0 1 0 0;
     0 0 0 0 1 0;
     0 0 0 0 0 1];                        # 状态转移矩阵
H = eye(6);                               # 转换矩阵,由于先验位置和速度和后验位置和速度都在同一个坐标系且都分解到xyz三轴的分量表示的所以方向相同
U = [1/2*dt^2; 1/2*dt^2; 1/2*dt^2; dt; dt; dt];                     # 输入控制矩阵
Q = [0.01 0 0 0 0 0;
     0 0.01 0 0 0 0;
     0 0 0.01 0 0 0;
     0 0 0 0.000001 0 0;
     0 0 0 0 0.000001 0;
     0 0 0 0 0 0.000001;];                  # 过程噪声协方差，估计一个
##
P = eye(6);                             # 初始值为 1（可为非零任意数）

##
x = zeros(6, N);                        # 存储滤波后的数据，分别为位移、速度信息
k = 1;                                  # 采样点计数

# ================= 卡尔曼滤波过程 ==================
for t = dt:dt:100
    k = k + 1
    x(:,k) = A * x(:,k-1) + U*aa(k);            # 卡尔曼公式1
    P = A * P * A' + Q;                         # 卡尔曼公式2
    K = P*H' * inv(H*P*H' + R);                 # 卡尔曼公式3
    x(:,k) = x(:,k) + K * (z(:,k)-H*x(:,k));    # 卡尔曼公式4
    P = (eye(6)-K*H) * P;                       # 卡尔曼公式5
end

# ==================== 结果绘图 =====================
figure(1);
plot3(z(1,:), z(2,:), z(3,:), 'b.');hold on;                   # GPS测量数据
plot3(x(1,:), x(2,:), x(3,:), 'r.');                           # 滤波后数据
plot3(s(1,:), s(2,:), s(3,:), 'g.');                               # 绘制真实值
legend('滤波前','滤波后','理想值');              # 标注
axis equal
xlabel('x: m');
ylabel('y: m');
zlabel('z: m');

figure(2);
plot(T, z(1,:),'b.');hold on;                   # GPS测量数据
plot(T, x(1,:),'r.');                           # 滤波后数据
plot(T, s(1,:),'k-');                               # 绘制真实值
legend('滤波前Px','滤波后Px','理想值Px');              # 标注
xlabel('时间: s');
ylabel('距离: m');hold off;

figure(3);
plot(T, z(2,:),'b.');hold on;                   # GPS测量数据
plot(T, x(2,:),'r.');                           # 滤波后数据
plot(T, s(2,:) ,'k-');                               # 绘制真实值
legend('滤波前Py','滤波后Py','理想值Py');              # 标注
xlabel('时间: s');
ylabel('距离: m');hold off;

figure(4);
plot(T, z(3,:),'b.');hold on;                   # GPS测量数据
plot(T, x(3,:),'r.');                           # 滤波后数据
plot(T, s(3,:) ,'k-');                               # 绘制真实值
legend('滤波前Pz','滤波后Pz','理想值Pz');              # 标注
xlabel('时间: s');
ylabel('距离: m');hold off;

#######################################

figure(5);
plot3(z(4,:), z(5,:), z(6,:), 'b.');hold on;                   # GPS测量数据
plot3(x(4,:), x(5,:), x(6,:), 'r.');                           # 滤波后数据
plot3(s(4,:), s(5,:), s(6,:), 'g.');                               # 绘制真实值
legend('滤波前','滤波后','理想值');              # 标注
axis equal
xlabel('x: m');
ylabel('y: m');
zlabel('z: m');

